//
// Created by DELL on 2023/12/4.
//

#include "auto_superc.h"

int Auto_Sc_Flag = 0;
int Auto_Power_Flag = 0;
float limit_power;

void SC_Auto_Enable(void){
    if(superCap.scFeedback.V_SC > SC_AUTO_ENABLE)
        Auto_Sc_Flag = WORK;

    else
        Auto_Sc_Flag = DISWORK;

    if ((GetErrorState() >> SUPERC_LOST_COUNT) & 0x0001) {
        Auto_Sc_Flag = DISWORK;
    }

    float Power_Use = superCap.power_fb.p_wheel;
    limit_power = judge_rece_mesg.game_robot_state_data.chassis_power_limit;
    float power_buffer = judge_rece_mesg.power_heat_data.chassis_power_buffer;
    float temp = limit_power - Power_Use;
    ///这个阈值暂时是随便给的，还需要进行测试才能更好地给出来
    if(temp < 5){
        Auto_Power_Flag = WORK;
    }
    else
        Auto_Power_Flag = DISWORK;

    if(Auto_Sc_Flag && Auto_Power_Flag){
        chassisControlData.disCharging_Auto = WORK;
        if(power_buffer < LIMIT_POWER_BUFFER)
            chassisControlData.disCharging_Auto = DISWORK;
    }
    else{
        chassisControlData.disCharging_Auto = DISWORK;
    }
    if(chassisControlData.superFlag == WORK)
        chassisControlData.disCharging_Auto = DISWORK;
}